
#include "RRT.h"
#include "../Utils/Vector2D.h"
#include "../Utils/Util.h"
#include "../Utils/Timer.h"
#include "../Map.h"

#include <list>
#include <vector>
#include <GL/gl.h>
#include <cstdlib>
#include <ctime>
#include <iostream>
#include <queue>
#include <cassert>


RRT::RRT(Map *target_map, unsigned num_nodes) : 
    target_map(target_map),
    num_nodes(num_nodes),
    ship_size(0.15f) {
}

RRT::~RRT(){
    for(unsigned i = 0; i < all_nodes.size(); i++){
        delete all_nodes[i];
    }
}


std::vector<Vector2D> RRT::planPath(Vector2D from, Vector2D to){
    Vector2D cp, normal, tmp;
    if(target_map->collideSphere(from, 2.0f*ship_size, normal, tmp, cp)){
        from = cp;
    }

    if(target_map->collideSphere(to, 2.0f*ship_size, normal, tmp, cp)){
        to = cp;
    }


    //assert(!target_map->pointInsideObstacle(from, true, true));
    //assert(!target_map->pointInsideObstacle(to, true, true));

    std::vector<Vector2D> result;

    RRTNode *root = new RRTNode;
    root->parent = NULL;
    root->position = from;
    all_nodes.push_back(root);

    for(unsigned i = 0; i < num_nodes; i++){
        RRTNode *new_node = createNewNode();
        all_nodes.push_back(new_node);

        if(existsLOS(to, new_node->position, false)){
            result = buildSimplifiedPath(new_node);
            result.push_back(to);
            break;
        }
    }

    //result = shortenPath(result);
    result = padPath(result); 


    std::vector<Vector2D> validated_result = result;
/*
    validated_result.push_back(result[0]);

    if(result.size() > 2){
        for(unsigned i = 1; i < result.size()-2; i++){
            if(i < 10 || i > result.size()-11){
                if(target_map->collideSphere(result[i], 2.0f*ship_size, normal, tmp, cp)){
                    result[i] = cp;
                }

                validated_result.push_back(result[i]);
                std::vector<Vector2D> vs = validateLineSegment(result[i], result[i+1]);

                for(unsigned j = 1; j < vs.size()-1; j++){
                    validated_result.push_back(vs[j]);
                }
            }
            else{
                validated_result.push_back(result[i]);
            }
        }
        validated_result.push_back(result[result.size()-2]);
    }
    validated_result.push_back(result[result.size()-1]);
*/

/*
    glPushMatrix();
    glColor3f(0.2f, 0.6f, 0.2f);
    glBegin(GL_LINE_STRIP);
    for(unsigned i = 0; i < result.size(); i++){
        glVertex3f(result[i].x, result[i].y, 0.0f);
    }
    glEnd();
    glPopMatrix();
*/
    return validated_result;
}

void RRT::render(void){
    glPushMatrix();
    glColor3f(0.2f, 0.6f, 0.2f);
    glBegin(GL_LINES);
    for(unsigned i = 0; i < all_nodes.size(); i++){
        for(unsigned j = 0; j < all_nodes[i]->links.size(); j++){
            glVertex3f(all_nodes[i]->position.x, all_nodes[i]->position.y, 0.0f);
            glVertex3f(all_nodes[i]->links[j]->position.x, all_nodes[i]->links[j]->position.y, 0.0f);
        }
    }
    glEnd();
    glPopMatrix();
}


std::vector<Vector2D> RRT::shortenPath(std::vector<Vector2D> cur_path){
    for(unsigned i = 1; i < cur_path.size()-1; i++){
        float move = 0.001f;
        while(move < 1.0f){
            Vector2D new_pos = cur_path[i] + move*(cur_path[i-1] - cur_path[i]);
            if(!existsLOS(new_pos, cur_path[i+1], false)){ break; }

            cur_path[i] = new_pos;
            move *= 1.5f;
        }
    }

    return cur_path;
}


std::vector<Vector2D> RRT::padPath(std::vector<Vector2D> cur_path){
    std::vector<Vector2D> result;

    if(cur_path.size() == 0){ return result; }
    for(unsigned i = 0; i < cur_path.size()-1; i++){
        result.push_back(cur_path[i]);
        for(unsigned j = 1; j < 5; j++){
            result.push_back((j/5.0f)*cur_path[i+1] + (1.0f - j/5.0f)*cur_path[i]);
        }
    }
    result.push_back(cur_path.back());

    return result;
}


std::vector<Vector2D> RRT::buildSimplifiedPath(RRTNode *tail){
    std::list<Vector2D> tmp_result;
    while(tail != NULL){
        tmp_result.push_front(tail->position);
        tail = tail->parent;
    }

    std::vector<Vector2D> result;
    std::list<Vector2D>::iterator it = tmp_result.begin();
    while(it != tmp_result.end()){
        result.push_back(*it);
        ++it;
    }

    // Now go through the vertex list and cull out ones not needed.
    
    std::vector<Vector2D> simplified;
    simplified.push_back(result[0]);
    for(unsigned i = 1; i < result.size()-1; i++){
        if(!existsLOS(simplified.back(), result[i+1], false)){
            simplified.push_back(result[i]);
        }
    }

    simplified.push_back(result.back());
    return shortenPath(simplified);
}

RRTNode* RRT::findClosestLOSNode(Vector2D point){
    float closest_dist = 0.0f;
    RRTNode *closest_node = NULL;
    bool found = false;

    Vector2D tmp1, tmp2;

    for(unsigned i = 0; i < all_nodes.size(); i++){
        float dist = (point - all_nodes[i]->position).length();
        if((!found || dist < closest_dist) &&
            !target_map->collideRaySegment(point, all_nodes[i]->position, tmp1, tmp2, true, false) &&
            !target_map->collideRaySegment(point, all_nodes[i]->position, tmp1, tmp2, false)){
            found = true;
            closest_dist = dist;
            closest_node = all_nodes[i];
        }
    }

    return closest_node;
}

RRTNode* RRT::createNewNode(void){
    Vector2D point = generateRandomValidPoint();
    RRTNode *parent_node = NULL;

    while((parent_node = findClosestLOSNode(point)) == NULL){
        point = generateRandomValidPoint();
    }

    RRTNode *new_node = new RRTNode;
    new_node->position = point;
    new_node->parent = parent_node;
    new_node->links.push_back(parent_node);

    parent_node->links.push_back(new_node);

    return new_node;
}

bool RRT::existsLOS(Vector2D p1, Vector2D p2, bool tight_collision_space){
    Vector2D tmp1, tmp2;
    return !target_map->collideRaySegment(p1, p2, tmp1, tmp2, true, tight_collision_space);
}


Vector2D RRT::generateRandomValidPoint(void){
    Vector2D point = generateRandomPointInMap();
    while(target_map->pointInsideObstacle(point, true)){
        point = generateRandomPointInMap();
    }
    return point;
}

Vector2D RRT::generateRandomPointInMap(void){
    // We want to keep the point within constraint space of the map, so offset the map dimensions 
    // by the ship size, which defines the constraint offset.
    float x = Util::rand_interval(ship_size, target_map->getWidth()-ship_size)  + target_map->getMinCoord().x;
    float y = Util::rand_interval(ship_size, target_map->getHeight()-ship_size) + target_map->getMinCoord().y;
    return Vector2D(x, y);
}

std::vector<Vector2D> RRT::validateLineSegment(Vector2D start, Vector2D end){
    std::vector<Vector2D> fail_safe;
    fail_safe.push_back(start);
    fail_safe.push_back(end);

    Vector2D cp, normal, tmp;
    std::vector<Vector2D> result;
    result.push_back(start);

    unsigned tries = 0;
    while(target_map->collideRaySegment(start, end, normal, cp, true, false) && tries < 5){
        target_map->collideSphere(cp, 2.0f*ship_size, normal, tmp, cp);
        result.push_back(cp);
        start = cp;
        tries++;
    }

    if(tries >= 5){
        return fail_safe;
    }

    result.push_back(end);
    return result;
}

